home *** CD-ROM | disk | FTP | other *** search
/ Linux Cubed Series 8: LINUX Games / Linux Cubed Series 8 - LINUX Games.iso / games / video / fly8111-.000 / fly8111- / fly8 / mat.c < prev    next >
C/C++ Source or Header  |  1979-12-31  |  12KB  |  500 lines
  1. /* ------------------------------------ mat.c ------------------------------- */
  2.  
  3. /* This is part of the flight simulator 'fly8'.
  4.  * Author: Eyal Lebedinsky (eyal@ise.canberra.edu.au).
  5. */
  6.  
  7. /* Matrix and vector manipulation.
  8. */
  9.  
  10. #include "plane.h"
  11.  
  12. #undef Mident
  13. extern void FAR FASTCALL
  14. Mident (MAT m)
  15. {
  16.     m[0][1] = m[0][2] = m[1][0] = 
  17.     m[1][2] = m[2][0] = m[2][1] = 0;
  18.     m[0][0] = m[1][1] = m[2][2] = FONE;
  19. }
  20.  
  21. #undef Mxpose
  22. extern void FAR FASTCALL
  23. Mxpose (MAT m)
  24. {
  25.     register short    t;
  26.  
  27.     t = m[0][1];    m[0][1] = m[1][0];    m[1][0] = t;
  28.     t = m[0][2];    m[0][2] = m[2][0];    m[2][0] = t;
  29.     t = m[1][2];    m[1][2] = m[2][1];    m[2][1] = t;
  30. }
  31.  
  32. extern void FAR FASTCALL
  33. fMrotx (MAT m, register short s, register short c)
  34. {
  35.     register int    t;
  36.  
  37.     t = m[0][1];
  38.     m[0][1] = fmul(t,c) - fmul(m[0][2],s);
  39.     m[0][2] = fmul(t,s) + fmul(m[0][2],c);
  40.  
  41.     t = m[1][1];
  42.     m[1][1] = fmul(t,c) - fmul(m[1][2],s);
  43.     m[1][2] = fmul(t,s) + fmul(m[1][2],c);
  44.  
  45.     t = m[2][1];
  46.     m[2][1] = fmul(t,c) - fmul(m[2][2],s);
  47.     m[2][2] = fmul(t,s) + fmul(m[2][2],c);
  48. }
  49.  
  50. extern void FAR FASTCALL
  51. fMroty (MAT m, register short s, register short c)
  52. {
  53.     register int    t;
  54.  
  55.     t = m[0][0];
  56.     m[0][0] =  fmul(t,c) + fmul(m[0][2],s);
  57.     m[0][2] = -fmul(t,s) + fmul(m[0][2],c);
  58.  
  59.     t = m[1][0];
  60.     m[1][0] =  fmul(t,c) + fmul(m[1][2],s);
  61.     m[1][2] = -fmul(t,s) + fmul(m[1][2],c);
  62.  
  63.     t = m[2][0];
  64.     m[2][0] =  fmul(t,c) + fmul(m[2][2],s);
  65.     m[2][2] = -fmul(t,s) + fmul(m[2][2],c);
  66. }
  67.  
  68. extern void FAR FASTCALL
  69. fMrotz (MAT m, register short s, register short c)
  70. {
  71.     register int    t;
  72.  
  73.     t = m[0][0];
  74.     m[0][0] = fmul(t,c) - fmul(m[0][1],s);
  75.     m[0][1] = fmul(t,s) + fmul(m[0][1],c);
  76.  
  77.     t = m[1][0];
  78.     m[1][0] = fmul(t,c) - fmul(m[1][1],s);
  79.     m[1][1] = fmul(t,s) + fmul(m[1][1],c);
  80.  
  81.     t = m[2][0];
  82.     m[2][0] = fmul(t,c) - fmul(m[2][1],s);
  83.     m[2][1] = fmul(t,s) + fmul(m[2][1],c);
  84. }
  85.  
  86. extern void FAR
  87. Mobj (register OBJECT *p)
  88. {
  89.     build_mat (p->T,
  90.         p->sinx = SIN(p->a[X]),
  91.         p->cosx = COS(p->a[X]),
  92.         p->siny = SIN(p->a[Y]),
  93.         p->cosy = COS(p->a[Y]),
  94.         p->sinz = SIN(p->a[Z]),
  95.         p->cosz = COS(p->a[Z]));
  96. }
  97.  
  98. extern void FAR
  99. Myxz (MAT m, AVECT a)
  100. {
  101.     build_mat (m, SIN(a[X]), COS(a[X]), SIN(a[Y]), COS(a[Y]),
  102.             SIN(a[Z]), COS(a[Z]));
  103. }
  104.  
  105. extern void FAR
  106. VxMmul (VECT R, VECT V, MAT M)
  107. {
  108.     Mxpose (M);
  109.     VMmul (R, V, M);
  110.     Mxpose (M);
  111. }
  112.  
  113. extern void FAR
  114. Mmul (MAT m, MAT t)
  115. {
  116.     VECT    temp;
  117.  
  118.     VMmul (temp, m[0], t);    Vcopy (m[0], temp);
  119.     VMmul (temp, m[1], t);    Vcopy (m[1], temp);
  120.     VMmul (temp, m[2], t);    Vcopy (m[2], temp);
  121. }
  122.  
  123. extern void FAR
  124. Vscale (VECT a, VECT b, int scalar)
  125. {
  126.     a[X] = fmul (b[X], scalar);
  127.     a[Y] = fmul (b[Y], scalar);
  128.     a[Z] = fmul (b[Z], scalar);
  129. }
  130.  
  131. extern void FAR
  132. Vmuldiv (VECT a, VECT b, int m, int d)
  133. {
  134.     if (1 == d) {
  135.         if (1 == m)
  136.             Vcopy (a, b);
  137.         else {
  138.             a[X] = b[X] * m;
  139.             a[Y] = b[Y] * m;
  140.             a[Z] = b[Z] * m;
  141.         }
  142.     } else if (1 == m) {
  143.         a[X] = b[X] / d;
  144.         a[Y] = b[Y] / d;
  145.         a[Z] = b[Z] / d;
  146.     } else {
  147.         a[X] = muldiv (b[X], m, d);
  148.         a[Y] = muldiv (b[Y], m, d);
  149.         a[Z] = muldiv (b[Z], m, d);
  150.     }
  151. }
  152.  
  153. /* This routine builds the cosine matrix from the Euler angles.
  154.  *
  155.  *    cy*cz-sy*sx*sz    cy*sz+sy*sx*cz    -sy*cx
  156.  *    -cx*sz        cx*cz        sx
  157.  *    sy*cz+cy*sx*sz    sy*sz-cy*sx*cz    cy*cx
  158.  *
  159.  *    tt1 = cy*sz
  160.  *    tt2 = sy*cz
  161.  *    tt3 = cy*cz
  162.  *    tt4 = sy*sz
  163.  *
  164.  *    tt3-tt4*sx    tt1+tt2*sx    -sy*cx
  165.  *    -cx*sz        cx*cz        sx
  166.  *    tt2+tt1*sx    tt4-tt3*sx    cy*cx
  167. */
  168. extern void FAR
  169. cbuild_matyxz (MAT T, int sx, int cx, int sy, int cy, int sz, int cz)
  170. {
  171.     int    tt1, tt2, tt3, tt4;
  172.                         /* x = pitch (up)    */
  173.                         /* y = roll (c'wise)    */
  174.                         /* z = yaw (left)    */
  175.     tt1 = fmul (cy, sz);
  176.     tt2 = fmul (sy, cz);
  177.     tt3 = fmul (cy, cz);
  178.     tt4 = fmul (sy, sz);
  179.  
  180.     T[0][0] = tt3 - fmul (tt4, sx);
  181.     T[0][1] = tt1 + fmul (tt2, sx);
  182.     T[0][2] = -fmul (sy, cx);
  183.  
  184.     T[1][0] = -fmul (cx, sz);
  185.     T[1][1] = fmul (cx, cz);
  186.     T[1][2] = sx;
  187.  
  188.     T[2][0] = tt2 + fmul (tt1, sx);
  189.     T[2][1] = tt4 - fmul (tt3, sx);
  190.     T[2][2] = fmul (cy, cx);
  191. }
  192.  
  193. /* This routine extracts the Euler angles from the cosine matrix. In the case
  194.  * that the pitch is too high it attempts to recover by first observing that
  195.  * the (roll+-heading) is readily available and then estimating the roll and
  196.  * seting the heading.
  197.  *
  198.  * This is how the matrix is interpreted in the critical angles:
  199.  *
  200.  * rot(y)*rot(x)*rot(z):
  201.  *
  202.  *    .[0]        .[1]        .[2]
  203.  *
  204.  * [0].    cy*cz-sy*sz*sx    cy*sz+sy*cz*sx    -sy*cx    [0].
  205.  * [1].    -cx*sz        cx*cz        sx    [1].
  206.  * [2].    sy*cz+cy*sz*sx    sy*sz-cy*cz*sx    cy*cx    [2].
  207.  *
  208.  * If cx == 0 we have:
  209.  *
  210.  * [0].    cy*cz-sy*sz*sx    cy*sz+sy*cz*sx    0    [0].
  211.  * [1].    0        0        sx    [1].
  212.  * [2].    sy*cz+cy*sz*sx    sy*sz-cy*cz*sx    0    [2].
  213.  *
  214.  * which when sx == 1 is:
  215.  *
  216.  * [0].    c(y+z)          s(y+z)        0    [0].
  217.  * [1].    0        0        1    [1].
  218.  * [2].    s(y+z)        -c(y+z)        0    [2].
  219.  *
  220.  * and when sx == -1 is:
  221.  *
  222.  * [0].    c(y-z)        -s(y-z)        0    [0].
  223.  * [1].    0        0        -1    [1].
  224.  * [2].    s(y-z)        c(y-z)        0    [2].
  225.  *
  226.  *    .[0]        .[1]        .[2]
  227. */
  228.  
  229. extern void FAR
  230. Mangles (OBJECT *p, MAT m, AVECT a, ANGLE dy)
  231. {
  232.     ANGLE    t1;
  233.  
  234.     if (iabs(m[1][2]) >= FCON (0.999)) {        /* ~2.5 degrees    */
  235.         a[X] = ATAN (m[1][2], ihypot2d (m[2][2], m[0][2]));
  236.  
  237.         a[Y] += dy;
  238.         if ((m[2][2] < 0) != (iabs(a[Y]) > D90))
  239.             a[Y] += D180;
  240.         t1 = ATAN (m[2][0], m[0][0]);        /* roll+-dir    */
  241.         if (m[1][2] >= 0)            /* roll+dir    */
  242.             a[Z] = t1 - a[Y];        /* dir        */
  243.         else                    /* roll-dir    */
  244.             a[Z] = a[Y] - t1;        /* dir        */
  245.     } else {
  246.         a[X] = ASIN (m[1][2]);            /* pitch    */
  247.         a[Z] = ATAN (-m[1][0], m[1][1]);    /* dir        */
  248.         a[Y] = ATAN (-m[0][2], m[2][2]);    /* roll        */
  249.     }
  250.  
  251.     if (p) {
  252.         p->sinx = SIN(p->a[X]);
  253.         p->cosx = COS(p->a[X]);
  254.         p->siny = SIN(p->a[Y]);
  255.         p->cosy = COS(p->a[Y]);
  256.         p->sinz = SIN(p->a[Z]);
  257.         p->cosz = COS(p->a[Z]);
  258.     }
  259.  
  260. }
  261.  
  262. /* This function uses the current Euler angles and the angular
  263.  * velocities (p, q, r) to compute the new Euler angles. However, there is
  264.  * an artifact of the Euler angles which makes the roll/heading indication
  265.  * very jerky when the pitch is too close to the zenith/nadir. This is not
  266.  * just a problem with accuracy but when the nose traverses a line (great
  267.  * circle) that passes close to the zenith it actually will move rapidly
  268.  * through different headings. Since the entity that is mostly stable at
  269.  * this time is [heading-roll] (when diving it is [heading+roll] that is
  270.  * stable) then the roll varies rapidly as well.
  271.  *
  272.  * This makes a pitch ladder un-usable in that position. But not all is lost
  273.  * - this routine will stabilise the ladder (if requested) inside the (about)
  274.  * 2 degrees circle around the vertical. This can be done by fixing the
  275.  * roll (or the heading) and allowing the other angle to vary freely.
  276.  *
  277.  * Fixing the heading will mean that when climbing, executing a roll will
  278.  * cause the ladder to roll as expected (while the heading indicates the
  279.  * value as of the time this high pitch was entered). This gives s good
  280.  * visual cue for orientation awareness, however you do not know the true
  281.  * heading at which you will exit the maneuvre.
  282.  *
  283.  * Fixing the roll means that the ladder freezes at high pitch angles. The
  284.  * heading however will be correct, and if one rolls at a high pitch and
  285.  * then pulls 'up' to exit the vertical then the heading is correctly
  286.  * predicted. This is usefull if you want to exit at a prescribed heading.
  287. */
  288.  
  289. #define HOLDRADIUS    573        /* 2 degrees */
  290. #define BADRADIUS    57        /* 0.2 degrees */
  291.  
  292. extern void FAR
  293. Euler (OBJECT *p)
  294. {
  295.     int    cx, sy, cy, options, dy, dz;
  296.  
  297.     options = IS_PLANE(p) ? EE(p)->ladder : 0;
  298.  
  299.     cx = COS(p->a[X]);
  300.     cy = COS(p->a[Y]);
  301.     sy = SIN(p->a[Y]);
  302.  
  303.     p->dae[X] = fmul (p->da[X], cy) + fmul (p->da[Z], sy);
  304.     dy = dz = 0;
  305.  
  306.     if (cx >= BADRADIUS) {
  307.         p->dae[Z] = fmul (p->da[Z], cy) - fmul (p->da[X], sy);
  308.         if (iabs(p->dae[Z])>>1 >= (Uint)cx)        /* truncate */
  309.             p->dae[Z] = (int)(p->dae[Z] * (long)FONE / cx);
  310.         else
  311.             p->dae[Z] = fdiv (p->dae[Z], cx);
  312.         p->dae[Y] = p->da[Y] - fmul (p->dae[Z], SIN(p->a[X]));
  313.         if (cx < HOLDRADIUS && (options & LD_HOLD)) { /* too jumpy */
  314.             if (options & LD_HOLDROLL) {
  315.                 sy = fdiv (HOLDRADIUS-cx, HOLDRADIUS-BADRADIUS);
  316.                 if (iabs(p->a[Y]) > D90)
  317.                     dy = TADJ(D180-p->a[Y]); /* roll->180 */
  318.                 else
  319.                     dy = TADJ(-p->a[Y]);    /* roll->0 */
  320.                 dy = fmul (2*dy, sy);    /* gradual */
  321.                 cy = fmul (p->dae[Y], sy);
  322.                 if (p->a[X] > 0) {
  323.                     p->dae[Z] += cy;
  324.                     dz =  -dy;
  325.                 } else {
  326.                     p->dae[Z] -= cy;
  327.                     dz =   dy;
  328.                 }
  329.                 p->dae[Y] -= cy;    /* hold roll */
  330.             } else {
  331.                 if (p->a[X] > 0)
  332.                     p->dae[Y] += p->dae[Z];
  333.                 else
  334.                     p->dae[Y] -= p->dae[Z];
  335.                 p->dae[Z] = 0;        /* hold heading */
  336.             }
  337.         }
  338.     } else {
  339.         if (options & LD_HOLDROLL) {
  340.             p->dae[Y] = 0;            /* hold roll */
  341.             p->dae[Z] = -p->da[Y];
  342.         } else {
  343.             p->dae[Z] = 0;            /* hold heading */
  344.             p->dae[Y] =  p->da[Y];
  345.         }
  346.     }
  347. }
  348.  
  349. /*************** from here on, only old (obsolete) stuff *******************
  350. */
  351.  
  352. #if 0
  353. extern void FAR
  354. Mangles1 (MAT m, AVECT a)        /* obsolete */
  355. {
  356.     ANGLE    t1;
  357.     int    t2;
  358.  
  359.     t1 = ASIN (m[1][2]);                /* pitch +-pi/2 */
  360.     t2 = COS (t1);                    /* always +ve    */
  361.     if (iabs(t1) > (D90-D90/16)) {            /* 6 degrees    */
  362.         t2 = ihypot2d (m[1][0], m[1][1]);
  363.         a[X] = ATAN (m[1][2], t2);
  364.  
  365.         a[Y] = 0;
  366.         if (m[2][2] < 0)
  367.             a[Y] = D180 - a[Y];
  368.         t1 = ATAN (m[2][0], m[0][0]);        /* roll+-dir    */
  369.         if (m[1][2] >= 0)            /* roll+dir    */
  370.             a[Z] = t1 - a[Y];        /* dir        */
  371.         else                    /* roll-dir    */
  372.             a[Z] = a[Y] - t1;        /* dir        */
  373.     } else {
  374.         a[X] = t1;                /* pitch    */
  375.         a[Z] = -ATAN (m[1][0], m[1][1]);    /* dir        */
  376.         a[Y] = -ATAN (m[0][2], m[2][2]);    /* roll        */
  377.     }
  378. }
  379.  
  380. extern void FAR
  381. Mangles (MAT m, AVECT a)
  382. /*
  383.  * Extract angles from orientation matrix. The pitch is always in the range
  384.  * -90...+90 (cos is +ve). When pointing along the z coord the direction
  385.  * and roll are mixed, so we assume old roll stays and we extract new
  386.  * direction only. There is some problem with the resolution of the arcsin
  387.  * near this vertical direction (actualy, whenever sin is close to 1).
  388. */
  389. {
  390.     ANGLE    t1;
  391.     int    t2;
  392.  
  393.     t1 = ASIN (m[1][2]);                /* pitch +-pi/2 */
  394.     a[X] = t1;
  395. #if 1
  396.     t2 = COS (t1);                    /* always +ve    */
  397. #else
  398.     t2 = fmul(m[1][0],m[1][0])+fmul(m[1][1],m[1][1]); /* cx**2 */
  399. #endif
  400.     if (t2 < 64) {
  401. #if 0
  402.         if (m[0][2]) {
  403.             t2 = SIN (a[Y]);
  404.             t2 = fmul (m[1][2], iabs (t2));
  405.             a[X] = ATAN (t2, iabs (m[0][2]));
  406.         } else if (m[2][2]) {
  407.             t2 = COS (a[Y]);
  408.             t2 = fmul (m[1][2], iabs (t2));
  409.             a[X] = ATAN (t2, iabs (m[2][2]));
  410.         }
  411. #endif
  412. #if 0
  413.         if (m[1][0]) {
  414.             t2 = SIN (a[Z]);
  415.             t2 = fmul (m[1][2], iabs (t2));
  416.             a[X] = ATAN (t2, iabs (m[1][0]));
  417.         } else if (m[1][1]) {
  418.             t2 = COS (a[Z]);
  419.             t2 = fmul (m[1][2], iabs (t2));
  420.             a[X] = ATAN (t2, iabs (m[1][1]));
  421.         }
  422. #endif
  423.         t1 = ATAN (m[2][0], m[0][0]);        /* roll+-dir    */
  424. #if 0
  425.                         /* keep old roll a[Y]    */
  426.         if (a[X] >= 0)                /* roll+dir    */
  427.             a[Z] = t1 - a[Y];        /* dir        */
  428.         else                    /* roll-dir    */
  429.             a[Z] = a[Y] - t1;        /* dir        */
  430. #endif
  431. #if 0
  432.                         /* keep old dir a[Z]    */
  433.         if (a[X] >= 0)                /* roll+dir    */
  434.             a[Y] = t1 - a[Z];        /* roll        */
  435.         else                    /* roll-dir    */
  436.             a[Y] = t1 + a[Z] ;        /* roll        */
  437. #endif
  438. #if 1
  439.         a[Y] = 0;                /* zero roll    */
  440.         if (a[X] >= 0)                /* roll+dir    */
  441.             a[Z] = t1;            /* dir        */
  442.         else                    /* roll-dir    */
  443.             a[Z] = -t1;            /* dir        */
  444. #endif
  445.     } else {
  446.         a[Z] = -ATAN (m[1][0], m[1][1]);    /* dir        */
  447.         a[Y] = -ATAN (m[0][2], m[2][2]);    /* roll        */
  448.     }
  449. }
  450. #endif
  451.  
  452. #if 0
  453. }
  454.     ANGLE    t1;
  455.     int    t2, t3;
  456.  
  457.     t1 = ASIN(m[1][2]);                /* pitch +-pi/2*/
  458.     a[X] = t1;
  459.     t2 = COS (t1);                    /* always +ve    */
  460.     if (t2 < 1) {
  461.         t1 = ASIN (m[2][0]);            /* roll+-dir    */
  462.         if (m[0][0] < 0)
  463.             t1 = D180 - t1;
  464. #if 0
  465.         /* keep old roll a[Y]                    */
  466.         if (a[X] >= 0)                /* roll+dir    */
  467.             a[Z] = t1 - a[Y];        /* dir        */
  468.         else                    /* roll-dir    */
  469.             a[Z] = a[Y] - t1;        /* dir        */
  470. #else
  471.         /* keep old dir a[Z]                    */
  472.         if (a[X] >= 0)                /* roll+dir    */
  473.             a[Y] = t1 - a[Z];        /* toll        */
  474.         else                    /* roll-dir    */
  475.             a[Y] = t1 + a[Z] ;        /* roll        */
  476. #endif
  477.     } else {
  478.         if (m[1][0] > t2)
  479.             t3 = FONE;
  480.         else if (-m[1][0] > t2)
  481.             t3 = -FONE;
  482.         else
  483.             t3 = fdiv(m[1][0],t2);
  484.         a[Z] = -ASIN (t3);            /* dir        */
  485.         if (m[1][1] < 0)
  486.             a[Z] = D180 - a[Z];
  487.  
  488.         if (m[0][2] > t2)
  489.             t3 = FONE;
  490.         else if (-m[0][2] > t2)
  491.             t3 = -FONE;
  492.         else
  493.             t3 = fdiv(m[0][2],t2);
  494.         a[Y] = -ASIN (t3);            /* roll        */
  495.         if (m[2][2] < 0)
  496.             a[Y] = D180 - a[Y];
  497.     }
  498. }
  499. #endif
  500.